/* 
* @Author: obsidian
* @Date:   2015-12-16 09:43:22
* @Last Modified by:   obsidian
* @Last Modified time: 2016-04-25 20:52:00
*/

#include "GPSpath.h"
#include "driver/sd/file.h"
#include "module/GPS/GPS.h"
#include "module/pid/pid.h"
#include "driver/math/inc/math.h"
#include "pointpath.h"
#include "module/steer/steer.h"
#include "module/camera/camera.h"
#include "module/gpsteer/gpsteer.h"
#include "driver/led/inc/led.h"
#include "control/car.h"
#include "module/lcd/lcd_global.h"
#include "control/store/store.h"
#include "module/brake/brake.h"
#include "module/radar/radar.h"
#include "module/ledboard/ledboard.h"
#include "math.h"
#include "module/def.h"

void gpspath_read_sd(void);
void gps_path_update(void);
void gpspath_init(void);
void gpspath_save(void);
void timer_cfg(void);

struct  GPSpath gpspath = 
{
        .status            = 0,                 //status
        .current_index     = 0,                  //current_index
        .direction         = 0,                  //direction
        .target_angle      = 0,                  //target_radian
        .target_point      = {0,0},
        .distance_error    = 0,                  //distance_error
        .angle_error       = 0,                  //angle_error
        .mixed_error       = 0,                  //mixed_error
        .angle_k           = 4.5,                 //angle_k  4.5
        .distance_k        = 1.0,                 //distance_k  1.5
        .camera_k          = 1.5,          //camera_k
        .pre               = 20,                  //pre
        .base_pre          = 15,                  //base_pre
        .camera_center     = 159,                 //camera_center
        .camera_y          = 7250,
        .start_idx         = 50,//40 + 12,//2091, //50
        /*
                funtion
         */
        .init               = gpspath_init,
        .read_from_sd       = gpspath_read_sd,
        .gps_path_update    = gps_path_update,    //
        .timer_driver       = timer_cfg,
        .save               = gpspath_save,
};





static int Route[30]={0, 196, 223, 437, 464, 674, 701, 915, 942, 1152, 1247, 1400, 1576, 1717, 1765, 1799, 1850, 2069, 2131, 2525, 2727, 2917, 3118, 3249, 3367, 3848};
struct Path_point judge_direc(unsigned short index)
{
    struct Path_point   point           = {0};
    if(gps.direction == 2)
    {
        point.point.x = - path[index].point.x;
        point.point.y =   path[index].point.y;      
        point.target_radian = - path[index].target_radian;
    }
    else
    {
        point.point.x =  path[index].point.x;
        point.point.y =  path[index].point.y;
        point.target_radian = path[index].target_radian;        
    }
    return point;
}

void gps_path_update()
{
    unsigned short      index           = 0;
//  unsigned short      delta_index     = 0;
    float               angle_error     = 0;
    float               distance_error  = 0;
    float               tmp_len_min     = 0;
    float               tmp_len         = 0;
    signed short        min_len_index   = 0;
    float               mend            = 0;
    float               mixed_error     = 0;
    signed char         route_index     = 0;
    signed char         pre_point;
    unsigned short      route_start_index = 0;
    unsigned short      route_end_index   = 0;
    if(gpspath.status == OFF)
        {return;}





    tmp_len_min = get_point2point_length(judge_direc(gpspath.current_index).point, gps.front_point);
    min_len_index = gpspath.current_index;

    for(index = gpspath.current_index;index < gpspath.current_index + 5; index ++)
    {
        if(gpspath.current_index > PATH_LEN) {return;}
        
        tmp_len = get_point2point_length(judge_direc(index).point,gps.front_point);
        
        if(tmp_len < tmp_len_min)
        {
            min_len_index = index; 
            tmp_len_min = tmp_len; 
        }
    }

    /*
     pid f
     */
    if(pid.route_id < PID_NUM - 1)
    {
        if(min_len_index > Route[pid.route_id + 1])
        {
            pid.route_id ++;
            if(10 <= pid.route_id && pid.route_id <= 11)
            {
                pid.mend[pid.route_id ]   = pid.mend[pid.route_id - 1];
                pid.error[pid.route_id][0] = pid.error[pid.route_id - 1][0];
                pid.error[pid.route_id][1] = pid.error[pid.route_id - 1][1];
            }
            else
            {
                pid.pid_clear();
            }
        }        
    }

    gpspath.current_index = min_len_index;

    pre_point = (gps.speed * gpspath.pre / 1000.0) + gpspath.base_pre; 
    //pre_point = pre_point / 3;
    if(
        (pid.route_id >= 14)
      )
    {
        pre_point = pre_point / 3;
    }
  
    angle_error = (judge_direc(min_len_index + pre_point).target_radian - gps.radian) * 180 / PI;


    if(angle_error > 180)
    {
        angle_error = angle_error - 360;
    }
    else if(angle_error < - 180)
    {
        angle_error = angle_error + 360;
    }

    float min_len_radian = judge_direc(min_len_index).target_radian;
    if(min_len_radian == 0)
    {
        distance_error =judge_direc(min_len_index).point.x -  gps.front_point.x;
    }
    else if(min_len_radian > 0)
    {
        distance_error = get_point2line_length(gps.front_point, judge_direc(min_len_index).point,min_len_radian * 180.0 / PI);
    }
    else if(min_len_radian < 0)
    {
        distance_error = - get_point2line_length(gps.front_point, judge_direc(min_len_index).point,min_len_radian * 180.0 / PI);
    }

    if(gps.front_point.y > gpspath.camera_y && camera.offset != 0 && camera.status == 1)
    {
        distance_error = (camera.offset-gpspath.camera_center) * gpspath.camera_k;
        //distance_error = ledboard.getdiserr();
    }
    
//ledboard
    if(
        (
            gps.front_point.y > gpspath.camera_y 
        )
        && ledboard.status == 1)//使用灯板
    {
            distance_error = ledboard.getdiserr();
    }
    if(
        (
            (gpspath.current_index < 1700 && gpspath.current_index > 1600)
            || (gps.front_point.y > 1300 && gps.front_point.y < 6013)
            //|| (gps.front_point.y > 5147 && gps.front_point.y < 6013)
        )
        && ledboard.status == 1)//使用灯板
    {
        if(tmp_len_min < 70)
        {
            distance_error = ledboard.getdiserr();
        }
    }



//test    clear the distance error    75
    if(gpspath.current_index > 1400 && gpspath.current_index < 1500)
    {
        distance_error = 0;
    }



    if(gps.front_point.y > 11900){gpsteer.release();}

    //lvbo
    distance_error = (distance_error + 3.0 * gpspath.distance_error) / 4.0;
    

    mixed_error = gpspath.angle_k * angle_error + gpspath.distance_k  *  distance_error;

    gpspath.distance_error = distance_error;
    gpspath.target_angle =  judge_direc(min_len_index  + pre_point).target_radian * 180 / PI;
    gpspath.angle_error = angle_error;
    gpspath.mixed_error = mixed_error;
    gpspath.target_point.x = path[gpspath.current_index].point.x;
    gpspath.target_point.y = path[gpspath.current_index].point.y;
    mend = pid.get_pid(mixed_error);
    steer.set_angle(mend);

    //camera.calibrate();
    ledboard.calibrate();
    //radar.Radar_control(gps.front_point.y,gps.angle);
    
    brake.control(gps.front_point.y,radar.distanse_of_bigCar);////////////////////////

    static char a = 0;
    gpspath.ctl_cnt ++;
    //GPS_update()中如果车的gps发生变化就将gps.send_status置为1
    //以此来避免车不动的情况下一直写到sd卡或进行蓝牙发送
//  //if(gps.send_status == 1 )
//  {
//      a ++;
//      //if(a >= 2 && lcd.p_status->is_run)
//      {
//          a = 0;
//          store.save();
//      }
//      gps.send_status = 0;
//  }
}

static struct GPSpath dup_gpspath;

void gpspath_read_sd()
{
    unsigned int k = 0;
    car.strjoint("gpspath");
    f_open(&fil,car.path,FA_READ);
    if(fil.fs == 0){return;}
    f_read(&fil, (char *)&dup_gpspath, sizeof(struct GPSpath), &k);
    gpspath.angle_k = dup_gpspath.angle_k;
    gpspath.distance_k = dup_gpspath.distance_k;
    gpspath.camera_k = dup_gpspath.camera_k;
    gpspath.camera_center = dup_gpspath.camera_center;
    gpspath.pre = dup_gpspath.pre;
    gpspath.base_pre = dup_gpspath.base_pre;
    gpspath.start_idx = dup_gpspath.start_idx;
    //gpspath.camera_y = dup_gpspath.camera_y;
    file_close();   
}

void gpspath_init()
{                                                            
    gpspath.current_index   =  gpspath.start_idx;
    gps.start_point.x       = path[gpspath.current_index].point.x;
    gps.start_point.y       = path[gpspath.current_index].point.y;
    gps.start_angle = path[gpspath.current_index].target_radian * 180 / 3.1415;
    gps.rear_point.x = gps.start_point.x;
    gps.rear_point.y = gps.start_point.y;
    gps.base_radian = gps.start_angle * PI / 180.0;
    gps.front_point.x = gps.rear_point.x + gps.car_len * sin(gps.base_radian);
    gps.front_point.y = gps.rear_point.y + gps.car_len * cos(gps.base_radian);
}

void gpspath_save()
{
    unsigned int k = 0;
    car.strjoint("gpspath");
    f_open(&fil,car.path,FA_WRITE | FA_CREATE_ALWAYS);
    if(fil.fs == 0){return;}
    f_write(&fil, (char *)&gpspath, sizeof(struct GPSpath), &k);
    file_close();
}


void timer_cfg(void)
{
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    NVIC_InitTypeDef NVIC_InitStructure;
    NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn; 
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3; 
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3; 
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 
    NVIC_Init(&NVIC_InitStructure); 
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5 , ENABLE);
    TIM_TimeBaseStructure.TIM_Prescaler = 72-1; 
    TIM_TimeBaseStructure.TIM_Period = 2500; // 2500 - 2.5ms
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);
    TIM_ITConfig(TIM5,TIM_IT_Update,ENABLE);
    TIM_Cmd(TIM5,ENABLE);

}

static unsigned char update_count = 0;
void TIM5_IRQHandler(void)
{   
    TIM_ClearFlag(TIM5,TIM_IT_Update);
    gps.GPS_update();
    update_count ++;
    if(update_count >= 2)
    {
            gpspath.gps_path_update();
            update_count = 0;
    }
   
}

